#include <utils/topic.h>

#include "fusion_perception.h"
#include "global.h"
#include "ros_adapter.h"
#include "utils/utils.h"

namespace fusion_perception {

static std::vector<std::string> cameras {
    "camera0",
    "camera1",
    "camera2",
    "camera3",
    "camera4",
    "camera5",
    "camera6",
};

void RosAdapter::work(void)
{
    using namespace std::placeholders;

    auto& global = Global::getInstance();

    fusionPerceptionSptr_ = std::make_shared<FusionPerception>();
    fusionPerceptionSptr_->registerFusedResultCB(std::bind(&RosAdapter::onMultiFusedResult, this, _1));

    radarObjectReceiverSptr_ = global.getNode()->create_subscription<Objects, std::function<void(ObjectsSptr)>>(
        radarObjectsTopic, 10, std::bind(&RosAdapter::onObjectsFromMultiSensor, this, _1));
    radarFusedObjectReceiverSptr_ = global.getNode()->create_subscription<Objects, std::function<void(ObjectsSptr)>>(
        radarFusedObjectsTopic, 10, std::bind(&RosAdapter::onObjectsFromMultiSensor, this, _1));
    cameraFusedObjectReceiverSptr_ = global.getNode()->create_subscription<Objects>(
        cameraFusedObjectsTopic, 10, std::bind(&RosAdapter::onObjectsFromMultiSensor, this, _1));

    multiFusedObjectPublisher_ = global.getNode()->create_publisher<Objects>(multiFusedObjectsTopic, 10);
}

void RosAdapter::onObjectsFromMultiSensor(ObjectsSptr msg)
{
    if (msg == nullptr) {
        return;
    }

    if (msg->objects.empty()) {
        return;
    }

    std::unordered_map<std::string, ObjectSptrVector> objectSptrVectors;

    auto& global = Global::getInstance();

    for (auto begin = msg->objects.begin(); begin != msg->objects.end(); begin++) {
        auto object = std::make_shared<Object>(std::move(*begin));

        // if (object->header.frame_id == "camera6") {
        //     continue;
        // }

        if (std::abs(object->x) > 20 || object->y > 70 || object->y < -50) {
            continue;
        }

        if (global.isRecord) {
            object->header.stamp = global.getNode()->now();
        }

        objectSptrVectors[object->header.frame_id].emplace_back(object);
    }

    for (auto& i : objectSptrVectors) {
        fusionPerceptionSptr_->pushObjectSptrVector(i.first, i.second);
    }
}

void RosAdapter::onMultiFusedResult(const ObjectSptrVector& objectSptrVector)
{
    Objects objects;

    for (auto& i : objectSptrVector) {
        i->extend.feature.clear();
        objects.objects.emplace_back(*i);
    }

    multiFusedObjectPublisher_->publish(std::move(objects));
}
}